﻿using System;

namespace ColorSpace
{
    public static class Converter
    {
        public static void FromRGB24ToI420(int width, int height, byte[] data, int offset, byte[] target, int targetOffset)
        {
            var requiredSourceBytes = width*height*3;
            if (data.Length - offset < requiredSourceBytes)
                throw new ArgumentOutOfRangeException("data");

            var requiredTargetBytes = width*height*3/2;
            if (target.Length - targetOffset < requiredTargetBytes)
                throw new ArgumentOutOfRangeException("target");

            var stride = width * 3;
            var uOffset = targetOffset + width * height;
            var vOffset = uOffset + width * height / 2;
            var ci = 0;
            for (var y = 0; y < height; y += 2)
                for (var x = 0; x < width; x += 2)
                {
                    var xi = offset + 3 * x;
                    var yi = height - y - 1;
                    var r00 = data[yi * stride + xi];
                    var g00 = data[yi * stride + xi + 1];
                    var b00 = data[yi * stride + xi + 2];

                    var r01 = data[yi * stride + xi + 3];
                    var g01 = data[yi * stride + xi + 4];
                    var b01 = data[yi * stride + xi + 5];

                    var r10 = data[(yi - 1) * stride + xi];
                    var g10 = data[(yi - 1) * stride + xi + 1];
                    var b10 = data[(yi - 1) * stride + xi + 2];

                    var r11 = data[(yi - 1) * stride + xi + 3];
                    var g11 = data[(yi - 1) * stride + xi + 4];
                    var b11 = data[(yi - 1) * stride + xi + 5];

                    target[targetOffset + y * width + x] = (byte)(((66 * r00 + 129 * g00 + 25 * b00 + 128) >> 8) + 16);
                    target[targetOffset + y * width + x + 1] = (byte)(((66 * r01 + 129 * g01 + 25 * b01 + 128) >> 8) + 16);
                    target[targetOffset + (y + 1) * width + x] = (byte)(((66 * r10 + 129 * g10 + 25 * b10 + 128) >> 8) + 16);
                    target[targetOffset + (y + 1) * width + x + 1] = (byte)(((66 * r11 + 129 * g11 + 25 * b11 + 128) >> 8) + 16);

                    var cb00 = ((112 * r00 - 94 * g00 - 18 * b00 + 128) >> 8) + 128;
                    var cb01 = ((112 * r01 - 94 * g01 - 18 * b01 + 128) >> 8) + 128;
                    var cb10 = ((112 * r10 - 94 * g10 - 18 * b10 + 128) >> 8) + 128;
                    var cb11 = ((112 * r11 - 94 * g11 - 18 * b11 + 128) >> 8) + 128;

                    var cr00 = ((-38 * r00 - 74 * g00 + 112 * b00 + 128) >> 8) + 128;
                    var cr01 = ((-38 * r01 - 74 * g01 + 112 * b01 + 128) >> 8) + 128;
                    var cr10 = ((-38 * r10 - 74 * g10 + 112 * b10 + 128) >> 8) + 128;
                    var cr11 = ((-38 * r11 - 74 * g11 + 112 * b11 + 128) >> 8) + 128;

                    var cbAverage = (cb00 + cb01 + cb10 + cb11) / 4;
                    var crAverage = (cr00 + cr01 + cr10 + cr11) / 4;

                    target[uOffset + ci] = (byte)cbAverage;
                    target[vOffset + ci++] = (byte)crAverage;
                }
        }

        public static byte[] FromRGB24ToI420(int width, int height, byte[] data, int offset)
        {
            var result = new byte[width*height*2];
            FromRGB24ToI420(width, height, data, offset, result, 0);
            return result;
        }

        public static void FromYUY2ToI420(int width, int height, byte[] data, int offset, byte[] target, int targetOffset)
        {
            var requiredSourceBytes = width*height*2;
            if (data.Length - offset < requiredSourceBytes)
                throw new ArgumentOutOfRangeException("data");

            var requiredTargetBytes = width*height*3/2;
            if (target.Length - targetOffset < requiredTargetBytes)
                throw new ArgumentOutOfRangeException("target");

            var stride = width * 2;
            var uOffset = targetOffset + width * height;
            var vOffset = uOffset + width * height / 2;
            var ci = 0;
            for (var y = 0; y < height; y += 2)
            {
                for (var x = 0; x < width; x += 2)
                {
                    var xSource0 = offset + x * 2;
                    var xSource1 = offset + (x + 1) * 2;
                    var ySource0 = y * stride;
                    var ySource1 = (y + 1) * stride;

                    var yTarget0 = targetOffset + y * width;
                    var yTarget1 = targetOffset + (y + 1) * width;
                    // y00
                    target[x + yTarget0] = data[xSource0 + ySource0];
                    // y01
                    target[x + 1 + yTarget0] = data[xSource1 + ySource0];
                    // y10
                    target[x + yTarget1] = data[xSource0 + ySource1];
                    // y11
                    target[x + 1 + yTarget1] = data[xSource1 + ySource1];

                    // cb
                    target[uOffset + ci] = (byte)((data[xSource0 + 1 + ySource0] + data[xSource0 + 1 + ySource1]) / 2);
                    // cr
                    target[vOffset + ci++] = (byte)((data[xSource0 + 3 + ySource0] + data[xSource0 + 3 + ySource1]) / 2);
                }
            }
        }

        public static byte[] FromYUY2ToI420(int width, int height, byte[] data, int offset)
        {
            var result = new byte[width * height * 2];
            FromYUY2ToI420(width, height, data, offset, result, 0);
            return result;
        }

        public static void FromI420ToRGB24(int width, int height, byte[] data, int offset, byte[] target, int targetOffset)
        {
            var requiredSourceBytes = width*height*3/2;
            if (data.Length - offset < requiredSourceBytes)
                throw new ArgumentOutOfRangeException("data");

            var requiredTargetBytes = width*height*3;
            if (target.Length - targetOffset < requiredTargetBytes)
                throw new ArgumentOutOfRangeException("target");

            var yStride = width;
            var uStride = width / 2;
            var uOffset = offset + width * height;
            var vOffset = uOffset + width * height / 2;
            var i = 0;
            for (var yCord = 0; yCord < height; yCord++)
            {
                for (var xCord = 0; xCord < width; xCord += 2)
                {
                    var c1 = data[offset + yCord * yStride + xCord] - 16;
                    var c2 = data[offset + yCord * yStride + xCord + 1] - 16;
                    var d = data[uOffset + yCord / 2 * uStride + xCord / 2] - 128;
                    var e = data[vOffset + yCord / 2 * uStride + xCord / 2] - 128;

                    target[targetOffset + i++] = (byte)Math.Min(255, Math.Max(0, (298 * c1 + 409 * e + 128) >> 8));//r
                    target[targetOffset + i++] = (byte)Math.Min(255, Math.Max(0, (298 * c1 - 100 * d - 208 * e + 128) >> 8));//g
                    target[targetOffset + i++] = (byte)Math.Min(255, Math.Max(0, (298 * c1 + 516 * d + 128) >> 8));//b

                    target[targetOffset + i++] = (byte)Math.Min(255, Math.Max(0, (298 * c2 + 409 * e + 128) >> 8));//r
                    target[targetOffset + i++] = (byte)Math.Min(255, Math.Max(0, (298 * c2 - 100 * d - 208 * e + 128) >> 8));//g
                    target[targetOffset + i++] = (byte)Math.Min(255, Math.Max(0, (298 * c2 + 516 * d + 128) >> 8));//b
                }
            }
        }

        public static byte[] FromI420ToRGB24(int width, int height, byte[] data, int offset)
        {
            var result = new byte[width * height * 3];
            FromI420ToRGB24(width, height, data, offset, result, 0);
            return result;
        }
    }
}
